clc
clear
close all

x=[0;10;pi;0;0;0];
u=[0;0.05;0.05;0.05;0.05];

load zmena

param.a=1.446;
param.b=1.408;
param.c=1.4370;
param.I=1549.034;
param.m=2220;
param.g=9.81;
param.Tp=125;
Tp=param.Tp;
Ts=0.01;

[A,B,C,D]=carLin(x,u,param);

sys=c2d(ss(A,B,C,D),Ts);
time=0:Ts:Ts*(Tp-1);
[y,t]=lsim(sys,kron(ones(length(time),1),u.'),time,x);

A=sys.A;
B=sys.B;
sys_order=size(A,2);
input_order=size(B,2);
d=zeros((param.Tp)*sys_order+input_order,1);
D=zeros(param.Tp*sys_order,length(d));
%D(1:sys_order,1:sys_order)=eye(sys_order);
%D(1:sys_order,end-input_order+1:end)=-B;
for i=2:param.Tp;
    in=(i-1)*sys_order+1;
    D(in:in+sys_order-1,in:in+sys_order-1)=eye(sys_order);
    j=1;
    jn=(j-1)*sys_order+1;
    D(in:in+sys_order-1,jn:jn+sys_order-1)=-A^(i-1);
    pomoc=zeros(size(B));
    for l=1:(i-1)
        pomoc=pomoc-A^(l-1)*B;
    end
    D(in:in+sys_order-1,end-input_order+1:end)=pomoc;
end

t=0:sys.Ts:(sys.Ts*param.Tp);
[~,Y]=ode2(@(t,y)carFceSol(t,y,u,param),0:sys.Ts:sys.Ts*(param.Tp-1),x);
Y(isnan(Y))=0;

Y=Y.';
[m,n]=size(Y);
d(1:(param.Tp)*sys_order)=Y(1:m*n);
d(end-input_order+1:end)=u;
out=D*d;
y_lin=zeros(6,param.Tp);
y_lin(1:6*param.Tp)=out(1:6*param.Tp);
y_lin=y_lin.';

pred=y+y_lin;
vypo=Y.';

rozdil=pred-Y.';


max(max(abs(rozdil)))



